#include <inttypes.h>
#include <string.h>
#include "nmea_0183.h"
#include "navigationData.h"
#include <system.h>

NMEA_GPS::NMEA_GPS(DataInput* input) :
	dataInput(input), state(STATE_IDLE) {
}

void NMEA_GPS::convertResult() {
	navigationData.itod = nmeaData.itod; // the day of week is missing but we dont care.

	navigationData.position.latitude_d = (double) nmeaData.lat
			/ (double) 10000000.0;
	navigationData.position.longitude_d = (double) nmeaData.lon
			/ (double) 10000000.0;
	navigationData.position.altitude_f = nmeaData.alt / 1000.0;

	// in 0.1 km/h = 0.1/3.6 m/s = 1/36 m/s
	navigationData.groundCourseSpeed.course_f = nmeaData.heading / 10.0;
	navigationData.groundCourseSpeed.speed_f = (float) nmeaData.groundSpeed
			/ 36.0;

	navigationData.isValid = nmeaData.OK;

	navigationData.fixMode = nmeaData.fixMode;
	navigationData.numberOfSatellites = nmeaData.numberOfSatellites;
	navigationData.horizontalAccuracy_f = nmeaData.horizontalAccuracy / 1000.0;
	nmeaData.status = NavigationData::CONSUMED; // never update old data
	navigationData.systemTimestampMillis = getSystemTimeMillis();
}

NavigationData::DataStatus NMEA_GPS::task() {
	while (dataInput->available()) {
		parse(dataInput->read());
		if (nmeaData.status == NavigationData::NEWDATA) {
			convertResult();
			return NavigationData::NEWDATA;
		}
	}
	return NavigationData::CONSUMED;
}

void parseTime(uint8_t c, uint8_t* state, uint32_t* value) {
	static uint8_t shift;
	uint32_t seconds;
	uint32_t minutes;
	uint8_t i;
	if (c == '.') { // seconds decimal comma. We have parsed hhmmss. already, recompute this to decimal.
		*state = 1;
		shift = 2;
		// The 2 last digits are seconds.
		seconds = *value % 100L;
		// Remove from pure value.
		*value -= seconds;
		minutes = *value % 10000L;
		// Remove from pure value.
		*value -= minutes;
		// Hours are factor 10000 too many by position but factor 36000000 too little to be in millis. That is 360.
		*value *= 360L;
		// Shift to final weight: They were factor 100 too much but are each 60000 msec. That is factor 600.
		*value += minutes * 600L;
		// Convert seconds to millis.
		*value += seconds * 1000L;
	} else if (!*state) {
		// Decimal point not yet encountered.
		*value *= 10;
		*value += (c - '0');
	} else {
		// Decimal point has been encountered already.
		if (shift) {
			uint32_t digit = (c - '0');
			// Upshift first digit after decimal point 2 places, 1nd 1 place and 3rd 0 places.
			for (i = 0; i < shift; i++)
				digit *= 10;
			// and add to minutes.
			*value += digit;
			shift--;
		}
	}
}

void parseDegrees(uint8_t c, uint8_t* state, uint32_t* value,
		uint32_t* minutes) {
	static uint8_t shift;
	uint8_t i;
	if (c == '.') {
		*state = 1;
		shift = 6;
		// The 2 last digits are minutes.
		*minutes = *value % 100;
		// Remove from pure value.
		*value -= *minutes;
		// Shift to final weight 10E7
		*minutes *= 10000000L;
		// Value is already weigthed by *100 so upshift 2 places less.
		*value *= 100000L;
	} else if (!*state) {
		// Decimal point not yet encountered.
		*value *= 10;
		*value += (c - '0');
	} else {
		// Decimal point has been encountered already.
		if (shift) {
			uint32_t digit = (c - '0');
			// Upshift first digit after decimal point 6 places, 2nd 5 places etc.
			for (i = 0; i < shift; i++)
				digit *= 10;
			// and add to minutes.
			*minutes += digit;
			shift--;
		}
	}
}

void parseInt32(uint8_t c, uint8_t logweight, uint8_t *state, uint32_t* value) {
	uint8_t i;
	static uint8_t shift;
	if (c == '.') {
		for (i = 0; i < logweight; i++)
			*value *= 10;
		*state = 1;
		shift = logweight;
	} else if (!*state) {
		*value *= 10;
		*value += (c - '0');
	} else {
		uint32_t digit = (c - '0');
		if (shift > 0) {
			shift--;
			for (i = 0; i < shift; i++) {
				digit *= 10;
			}
			*value += digit;
		}
	}
}

void parseInt8(uint8_t c, uint8_t* value) {
	*value *= 10;
	uint8_t digit = (c - '0');
	*value += digit;
}

void NMEA_GPS::parseGPVTG(char c) {
	static uint8_t state;
	static uint32_t temp;
	if (c == ',') {
		commaindex++;
		switch (commaindex) {
		case 1:
			temp = 0;
			state = 0;
			break;
		case 7:
			temp = 0;
			state = 0;
			break;
		default:
			break;
		}
		return;
	} else {
		switch (commaindex) {
		case 1:
			parseInt32(c, 1, &state, &temp);
			nmeaData.heading = temp;
			break;
		case 7:
			parseInt32(c, 1, &state, &temp);
			nmeaData.groundSpeed = temp;
			break;
		default:
			break;
		}
	}
}

void NMEA_GPS::parseGPGGA(char c) {
	static uint8_t state;
	static uint32_t minutes;
	if (c == ',') {
		commaindex++;
		state = 0;
		switch (commaindex) {
		case 1:
			nmeaData.itod = 0;
			break;
		case 2:
			nmeaData.lat = 0;
			minutes = 0;
			break;
		case 3:
			nmeaData.lat += minutes / 60L;
			break;
		case 4:
			nmeaData.lon = 0;
			minutes = 0;
			break;
		case 5:
			nmeaData.lon += minutes / 60L;
			nmeaData.fixMode = 0;
			break;
		case 6:
			nmeaData.numberOfSatellites = 0;
			break;
		case 7:
			nmeaData.horizontalAccuracy = 0;
			break;
		case 8:
			nmeaData.alt = 0;
			break;
		default:
			break;
		}
		return;
	} else {
		switch (commaindex) {
		case 1:
			parseTime(c, &state, &nmeaData.itod);
			break;
		case 2:
			parseDegrees(c, &state, (uint32_t*) &nmeaData.lat, &minutes);
			break;
		case 3:
			if (c == 'S')
				nmeaData.lat = -nmeaData.lat;
			break;
		case 4:
			parseDegrees(c, &state, (uint32_t*) &nmeaData.lon, &minutes);
			break;
		case 5:
			if (c == 'W')
				nmeaData.lon = -nmeaData.lon;
			break;
		case 6:
			parseInt8(c, &nmeaData.fixMode);
			break;
		case 7:
			parseInt8(c, &nmeaData.numberOfSatellites);
			break;
		case 8:
			parseInt32(c, 3, &state, &nmeaData.horizontalAccuracy);
			break;
		case 9:
			parseInt32(c, 3, &state, (uint32_t*) &nmeaData.alt);
			break;
		default:
			break;
		}
	}
}

void NMEA_GPS::parseGPRMC(char c) {
	// We will ignore all data except OK/Warning as it already
	// comes in other messages.
	static uint8_t state;
	if (c == ',') {
		commaindex++;
		state = 0;
		switch (commaindex) {
		case 1:
			nmeaData.itod = 0;
			break;
		case 2:
			nmeaData.OK = false;
			break;
		default:
			break;
		}
		return;
	} else {
		switch (commaindex) {
		case 1:
			parseTime(c, &state, &nmeaData.itod);
			break;
		case 2:
			nmeaData.OK = (c == 'A');
			break;
		default:
			break;
		}
	}
}

uint8_t char2hexdigit(uint8_t c) {
	if (c >= 'A' && c <= 'F')
		return c - 'A' + 10;
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 10;
	return c - '0';
}

/*
 * Hmm. This is capable of parsing one char at a time but too slow for
 * calling from an interrupt.
 */
void NMEA_GPS::parse(char c) {
	switch (state) {
	case STATE_IDLE:
		if (c == '$') {
			state = STATE_READ_ID;
			dataindex = 0;
			checksum = 0;
		}
		break;
	case STATE_READ_ID:
		id[dataindex++] = c;
		checksum ^= c;
		if (dataindex == 5) {
			dataindex = 0;
			commaindex = 0;
			state = STATE_DATA;
			if (strcmp("GPVTG", id) == 0) {
				sentence = GPVTG;
				//printf("GPVTG\n");
			} else if (strcmp("GPGGA", id) == 0) {
				sentence = GPGGA;
				//printf("GPGGA\n");
			} /*else if (strcmp("GPRMC", id)==0) {
			 sentence = GPRMC;
			 //printf("GPRMC\n");
			 } */else {
				state = STATE_IDLE; // We are not listening to other than these messages.
			}
		}
		break;
	case STATE_DATA:
		if (c == '*') {
			state = STATE_CHECKSUM1;
		} else {
			checksum ^= c;
			switch (sentence) {
			case GPVTG:
				parseGPVTG(c);
				break;
			case GPGGA:
				parseGPGGA(c);
				break;
			case GPRMC:
				parseGPRMC(c);
				break;
			}
			dataindex++;
		}
		break;
	case STATE_CHECKSUM1:
		checksum -= char2hexdigit(c) << 4;
		state = STATE_CHECKSUM2;
		break;
	case STATE_CHECKSUM2:
		checksum -= char2hexdigit(c);
		if (checksum) {
			nmeaData.status = NavigationData::INVALID;
		} else {
			nmeaData.status = NavigationData::NEWDATA;
		}
		state = STATE_IDLE;
		break;
	}
}
